
#ifndef PINJOINT_H_
#define PINJOINT_H_

#include "Constraint.h"

#define PINJOINT_DEFAULT_SPRING_STIFFNESS	100.f
#define PINJOINT_DEFAULT_SPRING_DAMPING		0.05f

namespace p2 {

class PinJoint: public Constraint
{
DECLARE_RTT(PinJoint, Constraint);

public:
	/*
	 * flags
	 */
	enum {
		FLAGS_CLEAR = 0,
		FLAGS_ENABLE_LIMITS = (1<<0),
		FLAGS_ENABLE_MOTOR = (1<<1),
		FLAGS_ENABLE_FRICTION = (1<<2),
		FLAGS_ENABLE_SPRING = (1<<3),
		FLAGS_FLEXIBLE_LENGTH = (1<<4)
	};

public:
	inline PinJoint(Anchor const& a0, Anchor const& a1, real length, void* userData);
	virtual ~PinJoint();

	static PinJoint* create(Anchor const& a0, Anchor const& a1,
			real length, void* userData);

	inline uint32 getFlags() const;
	inline void setFlags(uint32 flags);
	inline void addFlags(uint32 flags);
	inline void removeFlags(uint32 flags);
	inline real getLength() const;
	inline void setLength(real length);
	inline void setSpring(real k, real b);
	inline void setMotor(real motorW, real motorTorque);
	inline real getMuA() const;
	inline void setFriction(real muA);
	inline real getMinLimit() const;
	inline real getMaxLimit() const;
	inline void setLimits(real minLimit, real maxLimit);

	/*
     * Constraint implementation
     */
    virtual void preStep(real64 dT);
    virtual bool solvePositions(real64 dT);
    virtual bool solveVelocities(real64 dT);

protected:
    uint32 _flags;
    /*
     * distance between the anchors
     */
    real _length;
    /*
     * angular mass
     */
    real _mA;
    /*
     * REF: http://en.wikipedia.org/wiki/Hooke's_law
     * REF: http://gafferongames.com/game-physics/spring-physics/
     * applied if FLAGS_ENABLE_SPRING is specified
     */
    real _k, _b;
    /*
     * friction coefficient
     * applied if FLAGS_ENABLE_FRICTION is specified
     */
    real _muA;
    /*
     * motor speed and torque
     * applied if FLAGS_ENABLE_MOTOR is specified; these are combined
     */
	real _motorW, _motorTorque;
	/*
	 * angular limits
	 * applied if FLAGS_ENABLE_LIMIT is specified
	 */
	real _minLimit, _maxLimit;
};

inline PinJoint::PinJoint(Anchor const& a0, Anchor const& a1, real length, void* userData)
:   Constraint(a0, a1, userData),
    _flags(FLAGS_CLEAR),
    _length(length),
    _mA(0.f),
    _k(PINJOINT_DEFAULT_SPRING_STIFFNESS), _b(PINJOINT_DEFAULT_SPRING_DAMPING),
    _muA(0.f),
    _motorW(0.f), _motorTorque(0.f),
    _minLimit(0.f), _maxLimit(0.f)
{
	ASSERTLOG(_length >= 0.f, "PinJoint::PinJoint");
}

inline uint32 PinJoint::getFlags() const
{
	return _flags;
}

inline void PinJoint::setFlags(uint32 flags)
{
	_flags = flags;
}

inline void PinJoint::addFlags(uint32 flags)
{
	_flags |= flags;
}

inline void PinJoint::removeFlags(uint32 flags)
{
	_flags &= ~flags;
}

inline real PinJoint::getLength() const
{
	return _length;
}

inline void PinJoint::setLength(real length)
{
	_length = length;
}

inline void PinJoint::setSpring(real k, real b)
{
    _k = Math<real>::Max(1.f, k);
    _b = Math<real>::Max(0.f, b);
	_flags |= FLAGS_ENABLE_SPRING;
}

inline void PinJoint::setMotor(real motorW, real motorTorque)
{
	_motorW = motorW;
	_motorTorque = motorTorque;
	_flags |= FLAGS_ENABLE_MOTOR;
}

inline real PinJoint::getMuA() const
{
	return _muA;
}

inline void PinJoint::setFriction(real muA)
{
	_muA = muA;
	_flags |= FLAGS_ENABLE_FRICTION;
}

inline real PinJoint::getMinLimit() const
{
	return _minLimit;
}

inline real PinJoint::getMaxLimit() const
{
	return _maxLimit;
}

inline void PinJoint::setLimits(real minLimit, real maxLimit)
{
	_minLimit = minLimit;
	_maxLimit = maxLimit;
	_flags |= FLAGS_ENABLE_LIMITS;
}

}

#endif	//PINJOINT_H_
